#include "common/newBeeRobot.h"

newBee_Robot::newBee_Robot()
{
    /*legs*/
#define LEG_OFFSET_X    0.22       //{b}下腿安装位置
#define LEG_OFFSET_Y    0.072
#define LEG_offset_Z    0
    _Legs[0]=new newBee2_Leg(0,Vec3(LEG_OFFSET_X,-LEG_OFFSET_Y,LEG_offset_Z));  //FR
    _Legs[1]=new newBee2_Leg(1,Vec3(LEG_OFFSET_X,LEG_OFFSET_Y,LEG_offset_Z));  //FL
    _Legs[2]=new newBee2_Leg(2,Vec3(-LEG_OFFSET_X,-LEG_OFFSET_Y,LEG_offset_Z));  //RR
    _Legs[3]=new newBee2_Leg(3,Vec3(-LEG_OFFSET_X,LEG_OFFSET_Y,LEG_offset_Z));  //RL
#undef LEG_OFFSET_X
#undef LEG_OFFSET_Y
#undef LEG_offset_Z
    _feetPosNormalStand <<  0.2200,  0.2200, -0.2200, -0.2200,
                           -0.1920,  0.1920, -0.1920,  0.1920,
                           -0.3850, -0.3850, -0.3850, -0.3850;
    _robVelLimitX << -0.4, 0.4;
    _robVelLimitY << -0.3, 0.3;
    _robVelLimitYaw << -0.5, 0.5;
    _mass = 12.0;
    _pcb << 0.0, 0.0, 0.0;
    _Ib = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
}

newBee2_Robot::newBee2_Robot()
{
    /*legs*/
#define LEG_OFFSET_X    0.1881       //{b}下腿安装位置
#define LEG_OFFSET_Y    0.04675
#define LEG_offset_Z    0
    _Legs[0]=new newBee_Leg(0,Vec3(LEG_OFFSET_X,-LEG_OFFSET_Y,LEG_offset_Z));  //FR
    _Legs[1]=new newBee_Leg(1,Vec3(LEG_OFFSET_X,LEG_OFFSET_Y,LEG_offset_Z));  //FL
    _Legs[2]=new newBee_Leg(2,Vec3(-LEG_OFFSET_X,-LEG_OFFSET_Y,LEG_offset_Z));  //RR
    _Legs[3]=new newBee_Leg(3,Vec3(-LEG_OFFSET_X,LEG_OFFSET_Y,LEG_offset_Z));  //RL
#undef LEG_OFFSET_X
#undef LEG_OFFSET_Y
#undef LEG_offset_Z
    _feetPosNormalStand <<  0.1881,  0.1881, -0.1881, -0.1881,
                           -0.1300,  0.1300, -0.1300,  0.1300,
                           -0.3200, -0.3200, -0.3200, -0.3200;
    _robVelLimitX << -0.4, 0.4;
    _robVelLimitY << -0.3, 0.3;
    _robVelLimitYaw << -0.5, 0.5;
    _mass = 12.0;
    _pcb << 0.0, 0.0, 0.0;
    _Ib = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
}
